/*
	2008-2009 UMKC Robotics Team
	ATMega128 Code
	Updated: always updating
	
	Created By: Jared Bayne
	E-Mail:		JaredNBayne@gmail.com
	Created On:	February 1st, 2008

	Purpose:	This program is for the 2008-2009 IEEE
				Region 5 Student Robotics Competition.


*/

// This sends out what we get for the front left sensor.  It should be a number like 200+, when it gets higher, it's all yours - Dustin McBeth 4/4/09
#define DEBUG_EDGE_SENSOR 0

#define	DEBUG	0		//1=yes	0=no ... This doesn't even work! - Dustin McBeth 4/4/09
#define SIMULATE_COLOR	1


int Do_Wireless_Start=0;
#define REGULAR_SPEED	170  


#define	red			0
#define	green 		1
#define	yellow 		2
#define	blue		3

#define	full	1
#define	empty	0

int roomOrderToGoTo[4] = {0,0,0,0};
int isRoomSelectedAlready[4] = { 0,0,0,0};

#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include "LCD.h"
#include "USART.h"
#include "Initialize.h"
//#include "string.h"
#include "motors.h"
#include "Wheel_Encoders.h"
//#include "RC_Control.h"
#include "buttons.h"
//#include "digital_dist_sens.h"
#include "box_detectors.h"
//#include "ColorSensor.c"
#include "Line_Sensor.h"
#include "movement_funcs.h"
//#include "Movement.c" 	// Writen by Dustin with no clue what it is really suppose to DO!!!!!
#include "servo.h"
//#include "menus.c"
#include "ColorSensor.c"


char title[] = "UMKC 2009 Robot";
char instr[] = "press any btn...";

#define LED	   GET_BIT(PORTA).bit6	//Notice, output uses PORTB

int main (void) 
{

	/* Initializations */
	ports_init();
	usart_init(0);
	usart_init(1);
	init_lcd();
	wheel_encoders_init();
	motors_init();
	
	writeln(title); // "UMKC 2009 Robot"
	usart_writeln(title,1); // Communicate out side of robot saying: "UMKC 2009 Robot"
	lcdline2();	   
	write(instr); // Press any (b)u(t)to(n) .... 
	LED=ON;	// this is the On-board Atmega128 LED near the buttons
	
	//while(1)
	//self_calibrate_edge_detectors();	//Calibrate, then wait for another button press
	//arm_up();
	//writeln("Plan B:");
	//writeln("Press any button");
	lcdline2();
	//write("Press any button");
	write("hello!");
	//write("to start robot!");
	wait_for_button_press();	//ALWAYS KEEP THIS LINE, will pause robot at beginning - Dustin McBeth 4/4/09
	
	while(1){
		read_color_sensor();
		_delay_ms(2000);
	}
	
	//start in room 1
	
	//arm_down();
	//_delay_ms(2000);
	//
	//while(1)
	//{
	//
	//	go_forward_to_edge(REGULAR_SPEED);
	//	gobackward(255);
	//	encoder_delay(25);
	//	goleft(REGULAR_SPEED);
	//	encoder_delay(20);
	//
	//
	//}
	//
	//
///*	arm_down();
	//
	//
	//
	//arm_down();
	//_delay_ms(800);
	//gobackward(REGULAR_SPEED);
	//encoder_delay(15);
	//
	//goleft(REGULAR_SPEED);
	//encoder_delay(9);
	//
	//go_forward_to_edge(REGULAR_SPEED);
	//goleft(REGULAR_SPEED);
	//encoder_delay(4);
	//
	//stop();
	//while(1);
//*/	
	//
	//
	////	arm_up();
////		wait_for_button_press();
	////	arm_down();
////		wait_for_button_press();
	////	arm_up();
	//
	//
	////starting bot in front of box
	//
	//arm_down();
	//_delay_ms(800);
	//gobackward(REGULAR_SPEED);
	//encoder_delay(15);
	//
	//goleft(REGULAR_SPEED);
	//encoder_delay(48);
	//
	//
	//goforward(REGULAR_SPEED);
	//encoder_delay(55);
	//stop();
	//
	////drop box in center
	//
	//arm_up();
	//arm_up();
	//_delay_ms(800);
	//
	////go to the next box
	//gobackward(REGULAR_SPEED);
	//encoder_delay(5);
	//
	//
	//goright(REGULAR_SPEED);
	//encoder_delay(12);
	//
	//stop();
	//
	//goforward(REGULAR_SPEED-60);
	//encoder_delay(90);
	//
	//stop();
	//
	//arm_up();
	//
	//stop();
	//arm_down();
	//_delay_ms(800);
	//
	//gobackward(REGULAR_SPEED);	//encoder_delay(15);
	//
	//goleft(REGULAR_SPEED);
	//encoder_delay(41);
	//
	//goforward(REGULAR_SPEED);
	//encoder_delay(50);
	//
	//stop();
	//arm_up();	//
	//
	//stop();
}



